/*
 * main.cpp
 *
 *  Created on: Oct 27, 2012
 *      Author: bouchier
 */
/*! \file main.cpp
 * \brief Header file for jRoverMotors - the file which does whole-system exercising of jRover's motors
 *
 * <H1>
 * Build Configuration
 * </H1>
 *
 * This project depends on the following peer projects being at the same directory level:
 * - CQEI2C
 * - Metro
 * - CQEIMEncoder
 * - qetime
 * - PID
 * - RCTest
 * - ControlledMotor
 * - JRoverSensors
 *
 * Edit the project properties as follows to reference them as includes, link objects, and referenced projects.
 *
 * Under C/C++ Build -> Settings, Tool Settings tab, TerkOS C++ Compiler group, Directories settings
 * add the following paths
 * to the terkos paths that are already there. This adds them to the include path for compilation
 * - ../../CQEI2C
 * - ../../Metro
 * - ../../CQEIMEncoder
 * - ../../qetime
 * - ../../PID
 * - ../../RCTest
 * - ../../ControlledMotor
 * - ../../JRoverSensors
 * - ../../jrover_msgs/ros_lib/jrover
 *
 * Under C/C++ Build -> Settings, Tool Settings tab, TerkOS C++ Linker group, Miscellaneous settings, add
 * the following "other objects". This tells the linker to link to qetime.o & CQEI2C.o.
 * - ../../CQEI2C/Debug/CQEI2C.o
 * - ../../Metro/Debug/Metro.o
 * - ../../CQEIMEncoder/Debug/CQEIMEncoder.o
 * - ../../qetime/Debug/qetime.o
 * - ../../PID/Debug/pid.o
 * - ../../RCTest/Debug/RCTest.o
 * - ../../ControlledMotor/Debug/ControlledMotor.o
 * - ../../JRoverSensors/Debug/JRoverSensors.o
 * - ../../usonicRanger/Debug/usonicRanger.o
 *
 * In the Project References group, check the following projects. This builds them before the current project.
 * CQEI2C
 * Metro
 * CQEIMEncoder
 * qetime
 * PID
 * ControlledMotor
 *
 */

#define DEFAULT_TEST 6

// default command lines to run. Add new command lines here & comment out all but one
char *default_argv[] = {(char *)"jRoverMotors"};
int default_argc = 1;

#include <stdio.h>
#include <getopt.h>
#include <stdlib.h>   /* calloc, strtol */
#include <string.h>   /* String function definitions */
#include <unistd.h>
#include "qegpioint.h"
#include <qeanalog.h>
//#include "RCTest.h"
#include "Metro.h"
#include "CQEI2C.h"
#include "ControlledMotor.h"
#include "JRoverSensors.h"
#include "qetime.h"
#include "JRoverMotors.h"

using namespace std;

#define PRINT_RATE 6

// PID parameters for motors 1 - 16 (on servo ports & H-bridge for both servo & motor use)
#define DRIVE_HMOTOR_KP 15.0
#define DRIVE_HMOTOR_KI 2.0
#define DRIVE_HMOTOR_KD 1.0
#define DRIVE_SMOTOR_KP 10.0
#define DRIVE_SMOTOR_KI 1.0
#define DRIVE_SMOTOR_KD 0.0
#define SERVO_SMOTOR_KP 3.5
#define SERVO_SMOTOR_KI 2.0
#define SERVO_SMOTOR_KD 2.0
#define SERVO_HMOTOR_KP 10.0
#define SERVO_HMOTOR_KI 1.0
#define SERVO_HMOTOR_KD 0.0

extern ControlledMotor lfDrive;
extern ControlledMotor lfSteer;
extern ControlledMotor rfDrive;
extern ControlledMotor rfSteer;
extern ControlledMotor lmDrive;
extern ControlledMotor lmSteer;
extern ControlledMotor rmDrive;
extern ControlledMotor rmSteer;
extern ControlledMotor lbDrive;
extern ControlledMotor lbSteer;
extern ControlledMotor rbDrive;
extern ControlledMotor rbSteer;

CQEI2C i2c = CQEI2C();		// instantiate the I2C driver
CQEGpioInt &gpio = CQEGpioInt::GetRef();
JRoverMotors jRoverMotors = JRoverMotors();
JRoverSensors jRoverSensors = JRoverSensors();
CQEAnalog &analog = CQEAnalog::GetRef();

//struct RCChannel *dirRcc;
//struct RCChannel *speedRcc;
//struct RCChannel *estopRcc;

int printRate = PRINT_RATE;

//! Just initialize the first servo then exit
void jRoverTest1()
{
	// initialize motors up to the first servo. This must be done in order of the I2C chain
	printf("test 1\n");
	lmSteer.init(SERVO_SMOTOR_KP, SERVO_SMOTOR_KI, SERVO_SMOTOR_KD);
}

//! Just initialize the first two servos then exit
void jRoverTest2()
{
	// initialize motors up to the first servo. This must be done in order of the I2C chain
	printf("test 2\n");
	lmSteer.init(SERVO_SMOTOR_KP, SERVO_SMOTOR_KI, SERVO_SMOTOR_KD);
	lmDrive.init(DRIVE_SMOTOR_KP, DRIVE_SMOTOR_KI, DRIVE_SMOTOR_KD);
	lbDrive.init(DRIVE_HMOTOR_KP, DRIVE_HMOTOR_KI, DRIVE_HMOTOR_KD);
	lbSteer.init(SERVO_SMOTOR_KP, SERVO_SMOTOR_KI, SERVO_SMOTOR_KD);
}

//! initialize all motors
void jRoverTest3()
{
	printf("test 3\n");
	// initialize all motors, in order of the I2C chain
	jRoverMotors.initMotors();
}

#define SPEED_RUN_TIME 100
//! Ramp the drive motors
void jRoverTest4()
{
	float rampSpeeds[] = {5.0, 10.0, 15.0, 0.0, -5.0, -10.0, -15.0, 0.0};
	int speedRunTime;
	int rampCount = 8;
	int i;

	printf("test 4\n");
	jRoverMotors.initMotors();					// always start with this
	Metro metro = Metro(50);		// 50ms metronome

	// iterate over the ramp speeds, setting all motors to each value
	for (i=0; i<rampCount; i++) {	// for each speed setting
		setAllDriveSpeed(rampSpeeds[i]);	// set all motors to that speed
		speedRunTime = SPEED_RUN_TIME;		// set how long we'll run at this speed

		// while running at this speed, keep updating motors
		while(speedRunTime) {
			if (metro.check()) {		// if 50ms have passed & it's time to do the control loop
				jRoverMotors.updateAllMotors();			// run the PID loop
				speedRunTime--;
			}
		}
	}
}

void jRoverTest5()
{
	float driveAngVel[] = {0.0, 0.1, 0.3, 0.5, 0.0, -0.1, -0.3, -0.5, 0.0};
	int rampCount = 9;
	float linear = 10.0;
	int speedRunTime;
	int i;

	printf("test 5\n");
	jRoverMotors.initMotors();					// always start with this
	Metro metro = Metro(50);		// 50ms metronome

	// iterate over the angular velocities, setting all motors to each value
	for (i=0; i<rampCount; i++) {	// for each speed setting
		jRoverMotors.driveRover(linear, driveAngVel[i]);	// calculate speed for all motors & set them to run at it
		speedRunTime = SPEED_RUN_TIME;		// set how long we'll run at this speed

		// while running at this speed, keep updating motors
		while(speedRunTime) {
			if (metro.check()) {		// if 50ms have passed & it's time to do the control loop
				jRoverMotors.updateAllMotors();			// run the PID loop
				speedRunTime--;
			}
		}
	}
}

void jRoverTest6()
{
	float linear;
	float angular;
	float rcLinear, rcAngular;
	int estopWidth;

	printf("test 6\n");

	jRoverMotors.initMotors();					// always start with this

	// initialize the steering, speed & estop R/C structs & start interrupt monitoring
//	dirRcc = initRCTest(gpio, 1);		// initialize RC channel on DIO 0
//	speedRcc = initRCTest(gpio, 2);
//	estopRcc = initRCTest(gpio, 3);
	jRoverSensors.initSensors();	// and this

	// check motors every 50ms, and R/C every 250ms
	Metro metro = Metro(50);
	Metro metro250 = Metro(250);

	jRoverMotors.driveRover(0.0, 0.0);		// start off stopped
	while (1){
		if (metro250.check()) {
			// convert R/C values to desired speed range -20 - +20 ips & angle range +/-90
			rcLinear = 0.0 - (float)(jRoverSensors.getRcSpeed()-1480)/25.0;
			rcAngular = 0.0 - (float)(jRoverSensors.getRcDir()-1480)/1000.0;
			estopWidth = jRoverSensors.getRcEstop();
			printf("R/C control: rcLinear: %0.1f, rcAngular %0.1f, estopWidth: %d\n",
									rcLinear, rcAngular, estopWidth);

			// if estop (R/C off), pulsewidth is 0, don't run
			// else if R/C indicates we're trying to drive it, let R/C have control
			// else let ROS have control
			// where is subsumption when you need it?
			if (((rcLinear < -2.0) || (rcLinear > 2.0)) && (estopWidth < 1500) && (estopWidth > 900)) {
				linear = rcLinear;
				angular = rcAngular;
				printf("R/C control: linear: %0.1f, angular %0.1f (%d,%d), estopWidth: %d\n",
						linear, angular,
						jRoverSensors.getRcSpeed(), jRoverSensors.getRcDir(), estopWidth);
			} else if (estopWidth < 900 || estopWidth > 1500) {
				linear = angular = 0.0;
				printf("Estop prevents movement %d\n", estopWidth);
			} else if ((rcLinear < 2.0) && (rcLinear > -2.0)){
				linear = angular = 0.0;
				printf("No inputs request movement\n");
			}

			//jRoverMotors.driveRover(linear, angular);
		}
		if (metro.check()) {		// if 50ms have passed & it's time to do the control loop
			jRoverMotors.updateAllMotors();			// run the PID loop
		}
	}
}

void usage()
{
    printf("Usage: jRover [OPTIONS]\n"
    "\n"
    "Options:\n"
    "  -h, --help                   Print this help message\n"
    "  -t<testnum>					Run the specified test\n"
    "Tests:\n"
    "1: initialize the first steering motor\n"
    "2: initialize the rirst 2 steering motors\n"
    "3: initialize all  motors\n"
    "4: initialize all motors then ramp the drive motors"
    "5: init motors then drive with driveRover, setting correct motor speeds/angles"
    "6: R/C control"
    "\n"
    );
}


int main(int argc, char *argv[])
{
	int i;
	int testNum = DEFAULT_TEST;			// which test to execute

	// if no arguments (e.g. run from TerkIDE) use default arguments from list above
    if (argc == 1) {
    	printf("No args provided; using default: ");
    	for(i=0; i<default_argc; i++) {
    		printf("%s ", default_argv[i]);
    	}
    	printf("\n");
    	argc = default_argc;
    	argv = default_argv;
    }

    // parse options & arguments
    int option_index = 0, opt;
    static struct option loptions[] = {
        {"help",       no_argument,       0, 'h'},
         {0,0,0,0}
    };

    // initialize the encoder table. Encoder table entry should match motor #

    while(1) {
        opt = getopt_long (argc, argv, "hrt:",
                           loptions, &option_index);
        if (opt==-1) break;

        switch (opt) {
        case 'h': usage(); break;
        case 't': testNum = strtol(optarg, NULL, 10); break;
        default: usage(); exit(0);
        }
    }

    switch (testNum) {
    case 1: jRoverTest1(); break;
    case 2: jRoverTest2(); break;
    case 3: jRoverTest3(); break;
    case 4: jRoverTest4(); break;
    case 5: jRoverTest5(); break;
    case 6: jRoverTest6(); break;
    default: printf("Invalid test number\n"); exit(0);
    }
}
